/**
  ******************************************************************************
  * File Name          : g3p_driver.cpp
  * Description       : g3p_driver program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <tof_driver/nooploop.h>

std::string _port;
serial::Serial ser;

int _dev_num = 0;
std::string id_s[] = {"1","2","3","4","5","6","7","8"};
float dis[TOF_MAX];

unsigned char rxd[RXD_MAX]; 
unsigned char txd[TXD_MAX] = {0x57,0x10,0xFF,0xFF,0x00,0xFF,0xFF,0x63};                   

/* Private function prototypes -----------------------------------------------*/

/**
  * @brief  驱动主函数
  * @param
  * @retval
  */
int main(int argc, char**argv)
{
    ros::init(argc, argv, "tof_driver");
    ros::NodeHandle nh;
    ros::Rate loop_rate(10);

    sensor_msgs::Range _range[TOF_MAX];
    ros::Publisher tof_rpub[TOF_MAX];

    nh.getParam("/tof_driver/port", _port);
    nh.getParam("/tof_driver/dev_num", _dev_num);

    uint8_t i = 0;
    uint8_t c_id = 0;
    uint32_t _tmp = 0;
    uint32_t checksum = 0;
    float dis_tmp = 0;

    if(_dev_num>TOF_MAX)
    {
        _dev_num = TOF_MAX;
    }

    for(i=0; i<_dev_num; i++)
    {
        tof_rpub[i] = nh.advertise<sensor_msgs::Range>("/tof_sense/"+id_s[i]+"/range", 5);
    }

    try 
    { 
        //设置串口属性，并打开串口 
        ser.setPort(_port.c_str()); 
        ser.setBaudrate(115200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(5); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 

    while (ros::ok())
    {  
        txd[4] = c_id;
        txd[7] = c_id+0x63;
        //printf("txd:[");
        //for(i=0;i<TXD_MAX;i++)
        //{
        //    printf(" 0x%02x ",txd[i]);  
        //}
        //printf("]\r\n");
        ser.write(txd,TXD_MAX); 
        c_id = (c_id+1)%_dev_num;
        
        if (ser.available())
        {
            ser.read(rxd, RXD_MAX);
            checksum = 0;
            _tmp = 0;
            for(i=0;i<(RXD_MAX-1);i++)
            {
                checksum += rxd[i];  
            }
            //printf("rxd:[");
            //for(i=0;i<RXD_MAX;i++)
            //{
            //    printf(" 0x%02x ",rxd[i]);  
            //}
            //printf("]\r\n");
            if((rxd[0]==0x57)&&((uint8_t)checksum==rxd[15])&&(rxd[11]<=2))
            {
                _tmp = rxd[10];
                _tmp <<= 8;
                _tmp += rxd[9];
                _tmp <<= 8;
                _tmp += rxd[8];
                //printf("dis(%d):%d mm [%02x %02x %02x]\r\n",rxd[3], _tmp, rxd[10], rxd[9], rxd[8]);
            }
            else
            {
                _tmp = 5000;   
            }
            dis_tmp = _tmp;
            dis[rxd[3]] = dis_tmp/1000;
        }

        for(i=0; i<_dev_num; i++)
        {
            _range[i].header.frame_id = id_s[i];
            _range[i].radiation_type = 1;
            _range[i].field_of_view = 0.26;
            _range[i].min_range = 0.015;
            _range[i].max_range = 5.05;
            //printf("%d:%0.3f\r\n",i,dis[i]);
            _range[i].range = dis[i];
            tof_rpub[i].publish(_range[i]);
        }

        ros::spinOnce();
        loop_rate.sleep();
    }

    if(ser.isOpen())
    {
        ser.close();
    }
  
    return 0;
}
